/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStationEnhancedIO;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.SmartDashboard;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Ultrasonic;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import java.util.TimerTask;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Relay;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Team997Protobot extends SimpleRobot {

    private static final int kDigitalSlot = 4;
    private static final int kSolenoidSlot = 7;
    private static final int kAnalogSlot = 1;
    private Joystick m_leftStick = new Joystick(1);
    private Joystick m_rightStick = new Joystick(2);
    private TrackerDashboard tdb;
    // These are SpeedController objects, so the motors can use Jaguar or CANJaguar
    private SpeedController m_frontLeftMotor;
    private SpeedController m_frontRightMotor;
    private SpeedController m_rearLeftMotor;
    private SpeedController m_rearRightMotor;
    private Team997CubicDrive m_drivetrain;
    private LineFollower m_lineFollower = new LineFollower(kDigitalSlot, 5, 12, 7);
//    private SRF08Ultra m_SRF08_ultra = new SRF08Ultra(kDigitalSlot, SRF08Ultra.kRangeModeInches);
    private Ultrasonic m_SRF04_ultra = new Ultrasonic(kDigitalSlot, 8, kDigitalSlot, 9, Ultrasonic.Unit.kInches);
    private Hand m_robotHand = new Hand(kDigitalSlot, 1, 2, 14, 16, 10, 12, 14);
    private DigitalInput m_autoSwitch[] = new DigitalInput[4];
    private ArmCrioPID m_arm = new ArmCrioPID(kAnalogSlot, 2, 7, kDigitalSlot, 8, kDigitalSlot, 7, 4, 2);
    private Solenoid m_highGearSolenoid = new Solenoid(kSolenoidSlot, 1);
    private Solenoid m_lowGearSolenoid = new Solenoid(kSolenoidSlot, 2);
    private Solenoid m_deployOff = new Solenoid(kSolenoidSlot, 3);
    private Solenoid m_deployOn = new Solenoid(kSolenoidSlot, 4);
    private static final boolean kGearOff = false;
    private static final boolean kGearOn = !kGearOff;
    private static final boolean kLowGear = false;
    private static final boolean kHighGear = !kLowGear;
    private static final boolean kMinibotRetract = false;
    private static final boolean kMinibotDeploy = !kMinibotRetract;
    //Enabling this will disable standard dashboard.
    private static final boolean kEnableSmartDashboard = true;
    private static final boolean kUseCANDrive = false;
    private static final int kAutoSwitch1Channel = 1;
    private Compressor m_compressor = new Compressor(kDigitalSlot, 11, kDigitalSlot, 1);
    private DriverStationEnhancedIO m_enhancedIO = DriverStation.getInstance().getEnhancedIO();

    private void logSmartDashboard(String key, String value) {
        if (!kEnableSmartDashboard) {
            return;
        }
        int nRetCode = SmartDashboard.log(value, key);
        if (nRetCode != SmartDashboard.SUCCESS) {
            System.out.println("Error: " + SmartDashboard.diagnoseErrorCode(nRetCode));
        }
    }

    public void robotInit() {
        System.out.println("Build complete!");

        for (int i = 0; i < 4; i++) {
            m_autoSwitch[i] = new DigitalInput(kDigitalSlot, kAutoSwitch1Channel + i);
        }
        m_SRF04_ultra.setAutomaticMode(true);

        DriverStationPrinter.getInstance().println("Build complete!");
        if (kEnableSmartDashboard) {
            SmartDashboard.init();
        }
        logSmartDashboard("System State", "Init");
        if (kUseCANDrive) {
            try {
                m_frontLeftMotor = new CANJaguar(6, CANJaguar.ControlMode.kPercentVbus);
                m_frontRightMotor = new CANJaguar(3, CANJaguar.ControlMode.kPercentVbus);
                m_rearLeftMotor = new CANJaguar(5, CANJaguar.ControlMode.kPercentVbus);
                m_rearRightMotor = new CANJaguar(4, CANJaguar.ControlMode.kPercentVbus);

            } catch (CANTimeoutException e) {
                DriverStationPrinter.getInstance().println("CAN Init Error!");
            }

        } else {
            m_frontLeftMotor = new Jaguar(kDigitalSlot, 6);
            m_frontRightMotor = new Jaguar(kDigitalSlot, 3);
            m_rearLeftMotor = new Jaguar(kDigitalSlot, 5);
            m_rearRightMotor = new Jaguar(kDigitalSlot, 4);
        }
        m_drivetrain = new Team997CubicDrive(m_frontLeftMotor, m_rearLeftMotor, m_frontRightMotor, m_rearRightMotor);

        if (kEnableSmartDashboard) {
            //Start a timer task to update the SmartDashboard.
            java.util.Timer dashTimer = new java.util.Timer();
            dashTimer.schedule(new TimerTask() {

                public void run() {
                    updateSmartDashboard();
                }
            }, 0, 200);
        } else {
            tdb = new TrackerDashboard(m_highGearSolenoid);
        }
        java.util.Timer armTimer = new java.util.Timer();
        armTimer.schedule(new TimerTask() {

            public void run() {
                updateArm();
            }
        }, 0, 250);
        m_compressor.start();
    }

    public void disabled() {
        logSmartDashboard("System State", "Disabled");
    }

    public double round(double num, int digits) {
        for (int i = 0; i < digits; i++) {
            num *= 10;
        }
        num = (int) Math.floor(num + 0.5);

        for (int i = 0; i < digits; i++) {
            num /= 10;
        }
        return num;
    }

    public int getAutoState() {
        int nSwitchState = 0;
        for (int i = 0; i < 4; i++) {
            if (!m_autoSwitch[i].get()) {
                nSwitchState |= 1 << i;
            }
        }
        return nSwitchState;
    }

    /*
     * Autonomous Setting 1
     *
     * 1. Go forward on straight tape.
     * 2. Hang UBER on top, middle hook
     *
     * Autonomous Setting 2
     *
     * 1. Go forward on middle tape.
     * 2. Turn left to follow tape.
     * 3. Robot turns to face rack.
     * 4. Hang UBER on top hook.
     *
     * Autonomous Setting 3
     *
     * 1. Go forward on middle tape.
     * 2. Turn right to follow tape.
     * 3. Robot turns to face rack.
     * 4. Hang UBER on top hook.
     *
     * Autonomous Settings 4-6 inclusive
     *
     * Same thing as Settings 1-3 inclusive execpt for hang ring on middle rack.
     *
     * Autonomous Settings 7-9 inclusive
     *
     * Same thing as Settings 1-3 inclusive execpt for hang ring on lower rack.
     *
     * Autonomous Setting 0
     *
     * Do nothing.
     */
    public void autonomous() {
        getWatchdog().setEnabled(false);
        logSmartDashboard("System State", "Autonomous");

        int nAutoState = getAutoState();
        double distanceInInchesFromWallToStop;
        // Autonomous off
        if (nAutoState == 0) {
            return;
        }

        int nStraightLeftRightState = (nAutoState - 1) % 3;
        int nTopMiddleBottomState = (nAutoState - 1) / 3;

        double shoulderUpPosition = 0;
        double elbowUpPosition = 0;

        boolean turnLeft = false;
        switch (nStraightLeftRightState) {
            // Straight Line
            case 0:
                if (nAutoState == 1) {
                    // Top Row middle column of 3
                    shoulderUpPosition = 0.554;
                    elbowUpPosition = 0.562;
                    distanceInInchesFromWallToStop = 35;
                } else if (nAutoState == 4) {
                    // Middle Row middle column of 3
                    shoulderUpPosition = 0;
                    elbowUpPosition = 0;
                    distanceInInchesFromWallToStop = 50;
                } else {
                    // Bottom Row middle column of 3
                    shoulderUpPosition = 0;
                    elbowUpPosition = 0;
                    distanceInInchesFromWallToStop = 50;
                }
                break;
            case 1:
                turnLeft = true;
            default:
                if (nAutoState == 2 || nAutoState == 3) {
                    // Top Row right or left column of 3
                    shoulderUpPosition = 0.55;
                    elbowUpPosition = 0.562;
                    if (nAutoState == 2) {
                        distanceInInchesFromWallToStop = 39;
                    } else {
                        distanceInInchesFromWallToStop = 50;
                    }
                } else if (nAutoState == 5 || nAutoState == 6) {
                    // Middle Row right or left column of 3
                    shoulderUpPosition = 0;
                    elbowUpPosition = 0;
                    if (nAutoState == 5) {
                        distanceInInchesFromWallToStop = 50;
                    } else {
                        distanceInInchesFromWallToStop = 50;
                    }
                } else {
                    // Bottom Row right or left column of 3
                    shoulderUpPosition = 0;
                    elbowUpPosition = 0;
                    if (nAutoState == 8) {
                        distanceInInchesFromWallToStop = 50;
                    } else {
                        distanceInInchesFromWallToStop = 50;
                    }
                }
                break;
        }
        // Make sure we hold onto tube.

        m_robotHand.requestAutonomousState(Hand.kStateCollecting);
        m_robotHand.update();

        shiftGear(kLowGear);

        // We can immediately request the up position.
        for (int i = 0; i < 3; i++) {
            m_arm.setAutonomousArmPositions(shoulderUpPosition, elbowUpPosition);
        }

        // Drive until the tube is over the peg.
        while (isEnabled() && isAutonomous() && m_SRF04_ultra.getRangeInches() > distanceInInchesFromWallToStop) {
            m_lineFollower.Follow(m_drivetrain, turnLeft);
            m_robotHand.update();
        }

        if (nAutoState == 2) {
            try {
                Thread.sleep(300);
            } catch (InterruptedException e) {
                e.printStackTrace();
            }
            m_drivetrain.setLeftRightMotorOutputs(-.5, .5);
            try {
                Thread.sleep(100);
            } catch (InterruptedException e) {
                e.printStackTrace();
            }
        }

        // Release the tube.
        m_robotHand.requestAutonomousState(Hand.kStateReleasing);
        m_robotHand.update();

        m_arm.setAutonomousArmPositions(((shoulderUpPosition - Arm.kDownShoulderPosition) * 0.75) + Arm.kDownShoulderPosition, ((elbowUpPosition - Arm.kRetractedElbowPosition) * 0.85) + Arm.kRetractedElbowPosition);

        //Drive backwards
        m_drivetrain.setLeftRightMotorOutputs(.5, .5);
        try {
            Thread.sleep(500);
        } catch (InterruptedException e) {
            e.printStackTrace();
        }

        // Move arm down.
        m_arm.setAutonomousArmPositions(Arm.kDownShoulderPosition, Arm.kRetractedElbowPosition);

        //Drive backwards
        m_drivetrain.setLeftRightMotorOutputs(.5, .5);
        try {
            Thread.sleep(2000);
        } catch (InterruptedException ex) {
            ex.printStackTrace();
        }

        //Stop
        m_drivetrain.setLeftRightMotorOutputs(0, 0);
        m_robotHand.requestAutonomousState(Hand.kStateOff);
        m_robotHand.update();
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        boolean deploySwitchOn = false;
        shiftGear(kLowGear);
        getWatchdog().setEnabled(false);
        logSmartDashboard("System State", "TeleOp");
        m_robotHand.setJoystick(m_rightStick, 1, 4, 3, 2);

        while (isEnabled() && isOperatorControl()) {
            if (!m_leftStick.getRawButton(4) && !m_leftStick.getRawButton(5)) {
                m_drivetrain.tankDrive(m_leftStick, m_rightStick);
            } else if (m_leftStick.getRawButton(4)) {
                m_lineFollower.Follow(m_drivetrain, true);
            } else {
                m_lineFollower.Follow(m_drivetrain, false);
            }

            m_robotHand.update();

            if (m_leftStick.getRawButton(2)) {
                shiftGear(kLowGear);
            } else if (m_leftStick.getRawButton(3)) {
                shiftGear(kHighGear);
            }

            if (m_rightStick.getRawButton(5)) {
                m_drivetrain.setReverse(false);
            } else if (m_leftStick.getTrigger()) {
                m_drivetrain.setReverse(true);
            }

            //m_arm.update();

            try {
                // Minibot deploy switch.
                deploySwitchOn = m_enhancedIO.getDigital(2);
            } catch (DriverStationEnhancedIO.EnhancedIOException e) {
                DriverStationPrinter.getInstance().println("Waldo Deploy IO Error!");
                deploySwitchOn = false;
            }
            if (deploySwitchOn || m_leftStick.getRawButton(11) || m_rightStick.getRawButton(11)) {
                deploy(true);
            } else {
                deploy(false);
            }

//            if (m_rightStick.getRawButton(7)) {
//                DriverStationPrinter.getInstance().println("Distance: " + String.valueOf(m_SRF04_ultra.getRangeInches()));
//                m_SRF08_ultra.ping();
//                SmartDashboard.log(m_SRF08_ultra.getRange(), "Side Sensor Inches");
//                SmartDashboard.log(m_SRF08_ultra.getLight(), "Side Sensor light");
//                while (m_rightStick.getRawButton(7));
//            }

//            getWatchdog().feed();
        }
    }

    private void updateArm() {
        if (isEnabled() && isOperatorControl()) {
            m_arm.update();
        }
        if (kEnableSmartDashboard && (!isAutonomous())) {
            m_arm.updateSmartDashboard();
        }
    }

    private void updateSmartDashboard() {
        try {
            if (m_frontLeftMotor instanceof CANJaguar) {
                SmartDashboard.log(((CANJaguar) m_frontLeftMotor).getOutputCurrent(), "Front Left Current");
                SmartDashboard.log(((CANJaguar) m_frontRightMotor).getOutputCurrent(), "Front Right Current");
                SmartDashboard.log(((CANJaguar) m_rearLeftMotor).getOutputCurrent(), "Rear Left Current");
                SmartDashboard.log(((CANJaguar) m_rearRightMotor).getOutputCurrent(), "Rear Right Current");
            }
        } catch (CANTimeoutException e) {
            DriverStationPrinter.getInstance().println("CAN Current Read Error!");
        }
        SmartDashboard.log(getAutoState(), "Autonomous Setting");
        try {
            SmartDashboard.log(m_enhancedIO.getDigital(1), "Minibot Deploy Switch");
        } catch (DriverStationEnhancedIO.EnhancedIOException e) {
            DriverStationPrinter.getInstance().println("Waldo Deploy Read Error!");
        }
        m_robotHand.updateSmartDashboard();
        m_lineFollower.updateSmartDashboard();
        if (isOperatorControl() || (!isEnabled())) {
            SmartDashboard.log(m_SRF04_ultra.getRangeInches(), "Front Sensor Inches");
        }
    }

    /**
     * Shifts gears.
     * @param gear The gear (low vs. high)
     */
    private void shiftGear(boolean gear) {
        if (gear == kLowGear) {
            m_highGearSolenoid.set(kGearOff);
            m_lowGearSolenoid.set(kGearOn);
        } else {
            m_lowGearSolenoid.set(kGearOff);
            m_highGearSolenoid.set(kGearOn);
        }
    }

    private void deploy(boolean bDeploy) {
        if (bDeploy == false) {
            m_deployOff.set(true);
            m_deployOn.set(false);
        } else {
            m_deployOff.set(false);
            m_deployOn.set(true);
        }
    }
}
